using BepuUtilities;
using DemoRenderer;
using BepuPhysics;
using BepuPhysics.Collidables;
using System;
using DemoContentLoader;
using DemoUtilities;
using Demos.Demos.Characters;
using BepuUtilities.Collections;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace Demos.SpecializedTests
{
    public class CharacterTestDemo : Demo
    {
        CharacterControllers characters;
        public unsafe override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(20, 10, 20);
            camera.Yaw = MathHelper.Pi * -1f / 4;
            camera.Pitch = MathHelper.Pi * 0.05f;
            var masks = new CollidableProperty<ulong>();
            characters = new CharacterControllers(BufferPool);
            Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());

            var random = new Random(5);
            for (int i = 0; i < 8192; ++i)
            {
                ref var character = ref characters.AllocateCharacter(
                    Simulation.Bodies.Add(
                        BodyDescription.CreateDynamic(
                            new Vector3(250 * (float)random.NextDouble() - 125, 2, 250 * (float)random.NextDouble() - 125), new BodyInertia { InverseMass = 1 },
                            new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f)), 0.1f),
                            new BodyActivityDescription(-1))));

                character.CosMaximumSlope = .707f;
                character.LocalUp = Vector3.UnitY;
                character.MaximumHorizontalForce = 10;
                character.MaximumVerticalForce = 10;
                character.MinimumSupportContinuationDepth = -0.1f;
                character.MinimumSupportDepth = -0.01f;
                character.TargetVelocity = new Vector2(4, 0);
                character.ViewDirection = new Vector3(0, 0, -1);
                character.JumpVelocity = 4;
            }

            var origin = new Vector3(-3f, 0, 0);
            var spacing = new Vector3(0.5f, 0, -0.5f);
            // for(int i=0;i<12;++i)
            // {
            // for(int j=0;j<100;++j)
            // {
            // 无功位置=原点+新向量3(i,0,j)*间距;
            // 变量方向=Quaternion.CreateFromAxisAngle(Vector3.Normalize(new向量3(0.0001f)+新向量3((浮点)随机.NextDouble(),(浮点)随机.NextDouble(),(浮点)随机.NextDouble(),10*(浮点)随机.NextDouble());
            // Var Shape=new Box(0.1F+0.3F*(浮点)随机.NextDouble(),0.1F+0.3F*(浮点)随机.NextDouble(),0.1F+0.3F*(浮点)随机.NextDouble());
            // VAR可碰撞=新CollidableDescription(Simulation.Shapes.Add(shape),0.1F);
            // Shape.ComputeInertia(1,外惯性);
            // 变量选择=(i+j)%3;
            // 交换机(选择)
            // {
            // 案例0：
            // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(位置,方向),惯性,可碰撞,新的身体活动描述(0.01f));
            // 断线;
            // 案例1：
            // Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(位置,方向),可碰撞,新身体活动描述(0.01f));
            // 断线;
            // 案例2：
            // Simulation.Statics.Add(new StaticDescription(Position,Orientation,Collisible));
            // 断线;

            // }
            // }
            // }


            // Simulation.Statics.Add(新建StaticDescription(
            // New Vector3(0,-0.5f,0),Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1,0,1)),MathF.PI*0.00f),New CollidableDescription(Simulation.Shapes.Add(new Box(3000,1,3000)),0.1F);

            const int planeWidth = 256;
            const int planeHeight = 256;
            DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
                (int x, int y) =>
                {
                    Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2);
                    return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y);
                }, new Vector3(2, 1, 2), BufferPool, out var planeMesh);
            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));

            removedCharacters = new QuickQueue<CharacterController>(characters.CharacterCount, BufferPool);
        }

        QuickQueue<CharacterController> removedCharacters;
        int frameIndex;
        public override void Update(Window window, Camera camera, Input input, float dt)
        {
            var rotation = Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), 0.5f * dt);
            for (int i = 0; i < characters.CharacterCount; ++i)
            {
                ref var character = ref characters.GetCharacterByIndex(i);
                if ((frameIndex + i) % 128 == 0)
                    character.TryJump = true;
                var tangent = Vector3.Cross(new BodyReference(character.BodyHandle, Simulation.Bodies).Pose.Position, Vector3.UnitY);
                var tangentLengthSquared = tangent.LengthSquared();
                if (tangentLengthSquared > 1e-12f)
                    tangent = tangent / MathF.Sqrt(tangentLengthSquared);
                else
                    tangent = Vector3.UnitX;
                tangent *= 4;
                character.TargetVelocity.X = -tangent.X;
                character.TargetVelocity.Y = tangent.Z;
                // Matrix3x3.Transform(new Vector3(character.TargetVelocity.X,0,character.TargetVelocity.Y),Rotation,Out var RotatedVelocity);
                // character.TargetVelocity.X=rotatedVelocity.X;
                // character.TargetVelocity.Y=rotatedVelocity.Z;
            }
            // {
            // IF(Characs.CharacterCount>0)
            // {
            // var indexToRemove=frame Index%character s.CharacterCount;
            // removedCharacters.EnqueueUnsafely(characters.GetCharacterByIndex(indexToRemove));
            // characters.RemoveCharacterByIndex(indexToRemove);
            // }

            // var readdCount=(Int)(removedCharacters.Count*0.05F);
            // for(int i=0;i<readdCount;++i)
            // {
            // var toAdd=removedCharacters.Dequeue();
            // REF VAR CHARACTER=REF characters.AllocateCharacter(toAdd.BodyHandle,OUT VAR CHARACTER索引);
            // Character=toAdd;
            // }

            // }
            frameIndex++;
            base.Update(window, camera, input, dt);
        }
    }
}


